#include "MyRobot.h"

/**
 * This file includes the Autonomous() method, which
 * is run during the first 10 seconds of the match.
 */

void RobotDemo::Autonomous()
{
	
	myRobot.SetSafetyEnabled(false);
	//compressor.Start();
	//Initialization of Driver Station
	dsLCD = DriverStationLCD::GetInstance();

	
	while (IsAutonomous())
	{
		//encoder.Start();
		
		//print switch status
		if(enableAuton.Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Enable Auton");
			dsLCD->UpdateLCD();
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Disable Auton");
			dsLCD->UpdateLCD();
		}
		
		if(switchAuton.Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Auton 1");
			dsLCD->UpdateLCD();
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Auton 2");
			dsLCD->UpdateLCD();
		}
		
		if(enableAuton.Get())
		{
			if(switchAuton.Get())
			{
				dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "NORMAL AUTON");
				dsLCD->UpdateLCD();
				
				/*latch.Set(true);
				retractLatch.Set(false);
				launchers.Set(true);
				retractLaunchers.Set(false);	
				*/			
				
				//until we reach the target distance, go forward
				//while(encoder.Get() <= FORWARD_ROTATIONS)
				//{
				myRobot.TankDrive(-0.7, -0.73);
				Wait(3.5);
				//}
				
				//stop the encoder and the motors
				//encoder.Stop();
				myRobot.TankDrive(0.0, 0.0);
				
				motor2.Set(-0.5);
				
				//ensure latch is closed
				/*latch.Set(true);
				retractLatch.Set(false);
				Wait(1.0);
				*/
				
				motor2.Set(0.0);
								
				//fill the launchers with air and wait
				/*launchers.Set(false);
				retractLaunchers.Set(true);
				Wait(3.0);
				*/
				
				motor2.Set(0.0);
				//open the latch and wait for the shot
				/*latch.Set(false);
				retractLatch.Set(true);
				Wait(2.0);
				
				//retract the launchers and wait
				launchers.Set(true);
				retractLaunchers.Set(false);
				Wait(3.0);
				*/
				//close the latch
				//latch.Set(true);
				//retractLatch.Set(false);
			}
			else if(!switchAuton.Get())
			{
				dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "FLEE AUTON");
				dsLCD->UpdateLCD();
							
				//until we reach the target distance, go forward
				//while(encoder.GetDistance() <= LEAVE_ROTATIONS)
				//{
					//myRobot.TankDrive(-0.5, -0.5);
					//Wait(1.0);
					//myRobot.TankDrive(0.0, 0.0);

				//}
				//stop the encoder and motors
				//encoder.Stop();
			}
			else
			{
				
			}
		}
		else if(!enableAuton.Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "NO AUTON");
			dsLCD->UpdateLCD();
		}
		else
		{
			
		}
		
		
	}
	
	
	
	
}
